#include "src/misc.h"
#include <opencv2/core.hpp>
#include <benchmark/benchmark.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <Eigen/Dense>
#include <thread>
#include <chrono>
#include <ctime>
using namespace std;
using namespace cv;
using namespace Eigen;
using namespace benchmark;
const int MAXN = 100000;


void P2L(TGPoint& P, vector<TGPoint>& polyLine, int s, int e, double& ans) {
    ans = INT_MAX;
    for(int i = s; i < e; i ++) {
        ans = min(ans, pdist(P, {polyLine[i], polyLine[i+1]}).second);
    }
}

double withMultiThread(TGPoint& P, vector<TGPoint>& polyLine) {
    int den = 4;
    int n = polyLine.size();
    int m = n / den;
    vector<double>ans(den, INT_MAX);
    thread pool[den];
    for(int i = 0; i < den; i ++) {
        int s = i*m;
        int e = (i+1)*m;
        if(i+1 == den) e = n-1;
        pool[i] = thread(P2L, ref(P), ref(polyLine), s, e, ref(ans[i]));
        //  pool[i] = thread(P2L, P, polyLine, s, e, ref(ans[i]));
    }
    for(auto& p : pool) {
        p.join();
    }
    return *min_element(ans.begin(), ans.end());
}

double withMatrix(TGPoint& P0, vector<TGPoint>& pts) {
    int n = pts.size();
    MatrixXd P(1, 2);
    MatrixXd A(2, n);
    P << P0.x, P0.y;
    for(int i = 0; i < n; i ++) {
        A(0, i) = pts[i].x;
        A(1, i) = pts[i].y;
    }

    //计算定点P到折线段各个端点的最小距离
    auto _P = P.transpose() * MatrixXd::Ones(1, A.cols()); //2*A.cols()
    auto B = _P - A; //2*A.cols()
    auto P2P = B.cwiseProduct(B).colwise().sum().cwiseSqrt(); //点到折线端点的距离


    //计算定点到各个折线段垂足的最小距离
    auto A1 = A.block(0, 0, 2, A.cols()-1); //2*A.cols()-1
    auto A2 = A.block(0, 1, 2, A.cols()-1); //2*A.cols()-1
    auto tP = _P.block(0, 0, 2, A.cols()-1);
    auto V1 = tP - A1, V3 = tP - A2;
    auto V2 = A2 - A1;
    auto D1 = V1.cwiseProduct(V2).colwise().sum();
    auto D2 = V3.cwiseProduct(-V2).colwise().sum();
    auto Area = V1.row(0).cwiseProduct(V2.row(1)) - V1.row(1).cwiseProduct(V2.row(0));
    auto _A = A1 - A2;
    auto LineLength = _A.cwiseProduct(_A).colwise().sum().cwiseSqrt();
    auto P2L = Area.cwiseAbs().cwiseProduct(LineLength.cwiseInverse()); //点到直线的距离
    double retM = P2P.minCoeff();

    // auto _q = (D1.array() < 0).select(retM, P2L);
    // auto qq = (D2.array() < 0).select(retM, _q);
    // retM = qq.minCoeff();

    int ncol = P2L.cols();
    for(int i = 0; i < ncol; i ++) {
        if(D1(0, i) < 0 | D2(0, i) < 0) continue;
        retM = min(retM, P2L(0, i));
    }

    return retM;
}

void generateData(TGPoint& P, vector<TGPoint>& pts, RNG& rng) {
    vector<TGPoint> _pts;
    generateRandomPointSet(MAXN, rng, _pts);
    pts.reserve(MAXN);
    pts.emplace_back(_pts[0]);
    //去除重复点
    for(int i = 1; i < MAXN; i ++) {
        if(equal(_pts[i], pts.back())) continue;
        pts.emplace_back(_pts[i]);
    }
    P = pts.back(); pts.pop_back();
}

static void testBruteForce(State& state) {
    RNG rng(0);
    TGPoint P;
    vector<TGPoint> pts;
    generateData(P, pts, rng);
    for(const auto& e: state) {
        // state.PauseTiming();
        // generateData(P, pts, rng);
        // state.ResumeTiming();

        double ans = INT_MAX;
        P2L(P, pts, 0, pts.size()-1, ans);
    }
}

/*
规模: 10,000
时间: 762,492ns

*/
BENCHMARK(testBruteForce);

static void testMultiThread(State& state) {
    RNG rng(0);
    TGPoint P;
    vector<TGPoint> pts;
    generateData(P, pts, rng);
    for(const auto& e : state) {
        // TGPoint P;
        // vector<TGPoint> pts;
        // state.PauseTiming();
        // generateData(P, pts, rng);
        // state.ResumeTiming();

        withMultiThread(P, pts);
    }
}

/*
规模: 10,000
时间: 450,354ns  144,081ns（CPU TIME)

*/
BENCHMARK(testMultiThread);

static void testMatrix(State& state) {
    RNG rng(0);
    TGPoint P;
    vector<TGPoint> pts;
    generateData(P, pts, rng);

    for(const auto& e : state) {
        // TGPoint P;
        // vector<TGPoint> pts;
        // state.PauseTiming();
        // generateData(P, pts, rng);
        // state.ResumeTiming();

        withMatrix(P, pts);
    }
}

/*
规模: 10,000
时间: 16,807,785ns

*/
BENCHMARK(testMatrix);


BENCHMARK_MAIN();



















